/*
 * current_control.c
 *
 *  Created on: Nov 25, 2024
 *      Author: Vinson
 */

#include "current_control.h"
#include "utils.h"
#include "math.h"
#include "gpio.h"
extern gpio_t dbg_out2;

static float cal_Iq_Id(float ia,float ib,float angle_el)
{
	float I_alpha = 0.0f;
	float I_beta = 0.0f;

	/* Step1：克拉克逆变换. I_alpha = ia, I_beta = (2ib + ia) / √3 */
	I_alpha = ia;
	I_beta = _1_SQRT3 * ia + _2_SQRT3 * ib;

	//float ct = cos(angle_el);
	//float st = sin(angle_el);
	//float I_d = I_alpha * ct + I_beta * st;
	//float I_q = I_beta * ct - I_alpha * st;

	/* Step2：帕克逆变换. I_d = I_alpha * ct + I_beta * st */
	float I_q = I_beta * (float)cos(angle_el) - I_alpha * (float)sin(angle_el) ;
	return I_q;
}

float getDccurrent(float ia, float ib, float angle_el)
{
	return cal_Iq_Id(ia, ib, angle_el);
}
